#include "ParaCoordCalctor.h"

SG_FEMSOLVER_NAMESPACE_OPEN
    using SG::Algebra::Matrix;
    using SG::DataStructure::Common::Real;

    void Element::quad4ComputeIntegCoord (const SG::Algebra::Matrix& point, const SG::Algebra::Matrix& plane, SG::DataStructure::Common::Real& _OUT xi, SG::DataStructure::Common::Real& _OUT eta)
    {
        /** @brief    
         * 
         * 将 节点坐标带入插值公式得到：
         *  N1 x1 + N2 x2 + N3 x3 + N4 x4 = x
         * (1-xi)(1-eta) x1 + (1+xi)(1-eta) x2 + (1+xi)(1+eta) x3 + (1-xi)(1+eta) x4 = 4x
         * 且 
         * (-x1 +x2 + x3 -x4) * xi + (-x1 -x2 + x3 +x4) * eta + (x1 -x2 +x3 - x4) * eta*xi = 4x - (x1 + x2 + x3 + x4)
         * 得到 a11 xi + a12 * eta  + a13 * eta*xi = b1
         * 
         * 同理 a12 xi + a22 * eta = b2
         * a21 = -y1 +y2 + y3 -y4
         * a22 = -y1 -y2 + y3 +y4
         * b2 = 4y - (y1 + y2 + y3 + y4)
         */

        Real x  = point (0, 0);
        Real a1 = 4.0 * x - (plane (0, 0) + plane (1, 0) + plane (2, 0) + plane (3, 0));
        Real a2 = -plane (0, 0) + plane (1, 0) + plane (2, 0) - plane (3, 0);
        Real a3 = -plane (0, 0) - plane (1, 0) + plane (2, 0) + plane (3, 0);
        Real a4 =  plane (0, 0) - plane (1, 0) + plane (2, 0) - plane (3, 0);

        // eta
        Real y  = point (0, 1);
        Real b1 = 4.0 * y - (plane (0, 1) + plane (1, 1) + plane (2, 1) + plane (3, 1));
        Real b2 = -plane (0, 1) + plane (1, 1) + plane (2, 1) - plane (3, 1);
        Real b3 = -plane (0, 1) - plane (1, 1) + plane (2, 1) + plane (3, 1);
        Real b4 =  plane (0, 1) - plane (1, 1) + plane (2, 1) - plane (3, 1);

        // 求解二元二次方程的近似解
        xi  = (a3 * b1 - a1 * b3) / (a3 * b2 - a2 * b3);
        eta = (a1 * b2 - a2 * b1) / (a3 * b2 - a2 * b3);

        // 使用迭代法，获取形函数坐标
        SG::Algebra::Matrix xieta;
        
        while(1)
        {
            SG::Algebra::Matrix K(2,2);
            SG::Algebra::Matrix f(2,1,{a2 * xi + a3 * eta + a4 * xi * eta - a1,
                            b2 * xi + b3 * eta + b4 * xi * eta - b1});
            if(fabs(f(0,0))<1e-12 && fabs(f(1,0))<1e-12)
                break;

            K(0, 0) = a2 + a4 * eta;
            K(0, 1) = a3 + a4 * xi ;
            K(1, 0) = b2 + b4 * eta;
            K(1, 1) = b3 + b4 * xi ;
            auto Kinev = SG::Algebra::inverse2X2 (K);
            xieta = Kinev * f;
            xi  -= xieta(0, 0);
            eta -= xieta(1, 0);
        }
    }
SG_FEMSOLVER_NAMESPACE_CLOSE